/**************************************************************************************************
* Title: SumPlane.cpp
* Author: Gael Huber
* Description: Implements methods for a plane
**************************************************************************************************/
#include "SumMath.h"

/**************************************************************************************************
* Normalize plane (so that |a,b,c| == 1
**************************************************************************************************/
Plane* PlaneNormalize(Plane* pOut, const Plane* pP) {
	float l = 1.0f / sqrtf(pP->a*pP->a + pP->b*pP->b + pP->c*pP->c);
	pOut->a = pP->a * l;
	pOut->b = pP->b * l;
	pOut->c = pP->c * l;
	pOut->d = pP->d * l;
	return pOut;
}

/**************************************************************************************************
* Find intersection point between a plane and a line. If the line is parallel to the plane, null is
* returned.
**************************************************************************************************/
Vector3* PlaneIntersectLine(Vector3* pOut, const Plane* pP, const Vector3* pV1, const Vector3* pV2) {
	// Get the vector normal
	Vector3 n(pP->a, pP->b, pP->c);

	// Get the vector representing the length of the line segment
	Vector3 d;
	Vec3Sub(&d, pV2, pV1);

	// Find the dot product of dn
	float dn(Vec3Dot(&d, &n));

	// If dn == 0, return 0
	if(dn == 0.0f) {
		pOut = 0;
	} else {
		// dn acts as denominator for finding intersection point, so we must now find the numerator.
		// From this we can find the modifier amount as well
		dn = (pP->d - Vec3Dot(pV1, &n)) / dn;

		// pOut = pV1 + td
		Vec3Scale(&d, &d, dn);
		Vec3Add(pOut, pV1, &d);
	}

	return pOut;
}

/**************************************************************************************************
* Construct a plane from a point and a normal
**************************************************************************************************/
Plane* PlaneFromPointNormal(Plane* pOut, const Vector3* pPoint, const Vector3* pNormal) {
	// Generally speaking, d = p*n for any point on the plane
	pOut->a = pNormal->x;
	pOut->b = pNormal->y;
	pOut->c = pNormal->z;
	pOut->d = -Vec3Dot(pPoint, pNormal);
	return pOut;
}

/**************************************************************************************************
* Construct a plane from 3 points
**************************************************************************************************/
Plane* PlaneFromPoints(Plane* pOut, const Vector3* pV1, const Vector3* pV2, const Vector3* pV3) {
	Vector3 c, e1, e2;

	// e1 = v2 - v1
	Vec3Sub(&e1, pV2, pV1);

	// e2 = v3 - v1
	Vec3Sub(&e2, pV3, pV1);

	// Compute the cross product
	Vec3Cross(&c, &e2, &e1);

	// n = c / |c|
	Vector3 n(c / Vec3Length(&c));

	// From here, use point normal
	return PlaneFromPointNormal(pOut, pV1, &n);
}

/**************************************************************************************************
* Transform a plane by a matrix. The vector (a,b,c) must be normal. M should be the inverse 
* transpose of the transformation desired.
**************************************************************************************************/
Plane* PlaneTransform(Plane* pOut, const Plane* pP, const Matrix* pM) {
	// Interpret the plane as a homogeneous Vector4
	Vector4 p(&pP->a);
	
	// Transform the plane
	Vector4 pt;
	Vec4Transform(&pt, &p, pM);

	// Reinterpret pt to plane
	pOut->a = pt.x;
	pOut->b = pt.y;
	pOut->c = pt.z;
	pOut->d = pt.w;

	return pOut;
}